using UnityEngine;
using System.Collections;
using OpenCvSharp;
using System.IO;
//using OpenCvSharp.CPlusPlus;
using System;
using System.Runtime.InteropServices;

[RequireComponent(typeof(Renderer))]
public class DisplayColor : MonoBehaviour {
	
	public DeviceOrEmulator devOrEmu;
	private Kinect.KinectInterface kinect;
	public int threshold1,threshold2;
	private int fingerCounter,fingerCounterSecondHand,fingerCounter2;
	private Texture2D tex;
	public int offsety,offsetx;
	private CvMemStorage storage,polyStorage;
	private CvSeq<CvPoint> contours;
	private double largestContourAreaOpen;
	private bool toggle;
	private int depthImageWidth = 320;
	private int depthImageHeight = 240;
	private CvBox2D box,box2;
	public int fingersClosed,fingersTwoHands,fingersOpen;
	public float boxWidthTwoHands,closedHandsArea;
	public enum handStates {NONE,OPEN,CLOSED,TWOHANDSOPEN,TWOHANDSCLOSED,TWOHANDSJOINED,ONEOPENTWOCLOSED,ONECLOSEDTWOOPEN};
	public handStates handMode;
	
	private int perfCounter = 0;
	private float oldBoxCenterX1 = 0;
	private float oldBoxCenterX2 = 0;
	private float oldBoxCenterY1 = 0;
	private float oldBoxCenterY2 = 0;

	private float curBoxCenterX1 = 0;
	private float curBoxCenterY1 = 0;
	private float curBoxCenterX2 = 0;
	private float curBoxCenterY2 = 0;
	public int cursorOffsetX;
	public int cursorOffsetY;
	public float cursorScaleX;
	public float cursorScaleY;
	private int contourCounter =0;	
	private double ratio;
	public float alpha = 0.25f;
	private bool jitterOff = true;
	private float jitterThres = 4.0f;
	private IplImage img,imgGray,imgGrayDilated;
	byte[] test;
	Color32[] colorBuf;
	public GUIText gesturesName;
	// Use this for initialization
	void Start () {
		kinect = devOrEmu.getKinect();
		tex = new Texture2D(640,480,TextureFormat.ARGB32,false);
		renderer.material.mainTexture = tex;
		storage = new CvMemStorage(0);
		polyStorage = new CvMemStorage(0);
		largestContourAreaOpen = 0.0;
		toggle = false;
		fingersTwoHands = 8;
		fingersClosed = 0;
		fingersOpen = 3;
		fingerCounter = 0;
		fingerCounter2 = 0;
		fingerCounterSecondHand = 0;
		boxWidthTwoHands = 100.0f;
		contours = null;
		test = new byte[depthImageWidth*depthImageHeight*3];
		img = Cv.CreateImage(Cv.Size(depthImageWidth,depthImageHeight), BitDepth.U8 , 3);
		imgGray = new IplImage(img.Size,BitDepth.U8,1);
		imgGrayDilated = new IplImage(img.Size,BitDepth.U8,1);
		gesturesName.pixelOffset = new Vector2(Screen.width/2.0f, 50.0f + Screen.height/-2.0f);
		
	}
	
	// Update is called once per frame
	void Update () {
		
		if(perfCounter == 1)
		{
			perfCounter = 0;
			tex.SetPixels32(colorBuf);
			tex.Apply(false);
			return;
		}
		perfCounter++;
		
		if(Input.GetKey(KeyCode.J))
		{
			jitterOff = !jitterOff;
			if(jitterOff)
			{
				jitterThres = 4.0f;
			}
			else
			{
				jitterThres = 0.0f;
			}
		}
		contours = null;
		if (kinect.pollDepth()&& kinect.pollColor() )
		{
			
			int totalPixels = 640*480;
			bool flagNone = true;
			colorBuf = kinect.getColor();
			short[] depthData = kinect.getDepth();
			
			/*for (int pix = 0; pix < totalPixels; pix++)
			{
				byte byte1 = (byte)(depthData[pix]& 0x00FF);
				byte byte2 = (byte)((depthData[pix]& 0xFF00)>>8);
				int distance = (int) (byte1>>3|byte2<<5);
				int i = pix / 640 + offsetx;
				int j = pix % 640 - offsety;
				
				if(distance < threshold1 || distance > threshold2)
				{
					int idx = i*640+j;
					if((idx >= 0) && (idx < 640*480))
					{
						colorBuf[idx].r = 0x00;//Convert.ToByte((cb.pixels[pix].r*0.30)+(cb.pixels[pix].g* 0.59)+(cb.pixels[pix].b* 0.11));
						colorBuf[idx].g = 0x00;//Convert.ToByte((cb.pixels[pix].r*0.30)+(cb.pixels[pix].g* 0.59)+(cb.pixels[pix].b* 0.11));
						colorBuf[idx].b = 0x00;//Convert.ToByte((cb.pixels[pix].r*0.30)+(cb.pixels[pix].g* 0.59)+(cb.pixels[pix].b* 0.11));
					}
				}
				
	
			}*/
			
			for(int i = 0; i < 240; i++)
			{
				for(int j = 0; j < 320; j++)
				{
					int idx = (i*640+j)*2;
					int mipmapIdx = (i*320+j)*3;
					if(colorBuf[idx].r != 0x00 && colorBuf[idx].g != 0x00 && colorBuf[idx].b != 0x00)
					{
						colorBuf[idx].r = colorBuf[idx+1].r = colorBuf[idx+640].r = colorBuf[idx+641].r = 0xc8;
						colorBuf[idx].g = colorBuf[idx+1].g = colorBuf[idx+640].g = colorBuf[idx+641].g = 0xeb;
						colorBuf[idx].b = colorBuf[idx+1].b = colorBuf[idx+640].b = colorBuf[idx+641].b = 0xef;
						test[mipmapIdx] = test[mipmapIdx+1] = test[mipmapIdx+2] =0xff;
					}
					else
					{
						test[mipmapIdx] = test[mipmapIdx+1] = test[mipmapIdx+2] =0x00;
					}
					
				}				
			}		
			Marshal.Copy(test, 0, img.ImageData, depthImageWidth*depthImageHeight*3);
			img.Flip();
			
			
			Cv.CvtColor(img,imgGray,ColorConversion.RgbToGray);
			
			Cv.Dilate(imgGray,imgGrayDilated);
			
			Cv.FindContours(imgGrayDilated,storage,out contours);
			
			CvSeq<CvPoint> contourCopy = contours;
			
			CvSeq<CvPoint> largestContour = null;
			CvSeq<CvPoint> secondLargestContour = null;
			double result1 =0 , result2=0;

				
			while(contours!=null)
			{
				
				result1 = Math.Abs(Cv.ContourArea(contours,CvSlice.WholeSeq));
				if(result1 > result2) 
				{ 
					result2 = result1; 
					largestContour = contours;
				}
				contours = contours.HNext;
				contourCounter++;
			}
			
			
			if(largestContour!=null)
			{
				double largestContourArea = Math.Abs(Cv.ContourArea(largestContour,CvSlice.WholeSeq)) ;
				
				if(largestContourArea < 120.0)
				{

					handMode = handStates.NONE;
					gesturesName.text = "None";
					return;
				}
				result2 =0;
				while(contourCopy!=null)
				{
					result1 = Math.Abs(Cv.ContourArea(contourCopy,CvSlice.WholeSeq));
					if(result1 > result2 && result1< largestContourArea)
					{ 
						result2 = result1; 
						secondLargestContour = contourCopy;
					}
					contourCopy = contourCopy.HNext;
				}

				int[] hull = null;
				
				Cv.ConvexHull2(largestContour,out hull,ConvexHullOrientation.Clockwise);
				box = Cv.MinAreaRect2(largestContour);//make the hand largest contour
				CvPoint2D32f[] points = box.BoxPoints();

				float distance = 0.0f;
				CvSeq<CvConvexityDefect> defect =  Cv.ConvexityDefects(largestContour,hull);
				#region defect_points
				for(int i =0;i<defect.Total;++i)
				{
					//imgGrayDilated.DrawCircle(defect[i].Value.DepthPoint,10,new CvScalar(255));
//					if(defect[i].Value.Depth>9.0f)
//						print ("defect Depth "+defect[i].Value.Depth);
					if(defect[i].Value.Depth>10.0f && defect[i].Value.Depth<20.0f)
						fingerCounter++;
					else if(defect[i].Value.Depth>20.0f)
						fingerCounter2++;
				}
				#endregion
				#region two_hands
				if(secondLargestContour!=null && Math.Abs(Cv.ContourArea(secondLargestContour,CvSlice.WholeSeq))>500.0)
				{
					box2 = Cv.MinAreaRect2(secondLargestContour);
					CvPoint2D32f[] points2 = box2.BoxPoints();
					
					distance = (box.Center.X-box2.Center.X)*(box.Center.X-box2.Center.X)+(box.Center.Y-box2.Center.Y)*(box.Center.Y-box2.Center.Y);
					
					int[] hullSecondHand = null;
				
					Cv.ConvexHull2(secondLargestContour,out hullSecondHand,ConvexHullOrientation.Clockwise);
					CvSeq<CvConvexityDefect> defectSecondHand =  Cv.ConvexityDefects(secondLargestContour,hullSecondHand);
					#region defect_points
					for(int i =0;i<defectSecondHand.Total;++i)
					{
						if(defectSecondHand[i].Value.Depth>10.0f)
							fingerCounterSecondHand++;
					}
					#endregion
					
					if(fingerCounter <= fingersClosed && fingerCounterSecondHand <= fingersClosed)
					{
						handMode = handStates.TWOHANDSCLOSED;
						fingerCounter=0;
						fingerCounterSecondHand=0;
						gesturesName.text = "Two Hands Closed";
					}
					else if(fingerCounter <= fingersClosed)
					{
						if(box.Center.X <box2.Center.X)
						{
							handMode = handStates.ONEOPENTWOCLOSED;//ROPEN_LCLOSED;
							
						}
						else
						{
							
							handMode = handStates.ONECLOSEDTWOOPEN;//RCLOSED_LOPEN;
							
						}
						gesturesName.text = "One Hand Open Other Closed";
						fingerCounter=0;
						fingerCounterSecondHand=0;
					}
					else if(fingerCounterSecondHand <= fingersClosed)
					{
						if(box.Center.X <box2.Center.X)
						{
							handMode = handStates.ONECLOSEDTWOOPEN;//RCLOSED_LOPEN;
						}
						else
						{
							handMode = handStates.ONEOPENTWOCLOSED;//ROPEN_LCLOSED;
						}
						gesturesName.text = "One Hand Open Other Closed";
						fingerCounter=0;
						fingerCounterSecondHand=0;
					}
					else
					{
						handMode = handStates.TWOHANDSOPEN;
						fingerCounter=0;
						fingerCounterSecondHand=0;
						gesturesName.text = "Two Hands Open";
					}
					
					oldBoxCenterX1 = curBoxCenterX1;
					oldBoxCenterX2 = curBoxCenterX2;
					oldBoxCenterY1 = curBoxCenterY1;
					oldBoxCenterY2 = curBoxCenterY2;
					
					if(Mathf.Abs (oldBoxCenterX1-box.Center.X)>jitterThres)
						curBoxCenterX1 = box.Center.X;
					else
						curBoxCenterX1 = (box.Center.X+curBoxCenterX1)/2.0f;
				//curBoxCenterX1 = alpha*curBoxCenterX1 + (1-alpha)*box.Center.X;
					if(Mathf.Abs (oldBoxCenterY1-box.Center.Y)>jitterThres)
						curBoxCenterY1 = box.Center.Y;
					else
						curBoxCenterY1 = (box.Center.Y+curBoxCenterY1)/2.0f;
				//curBoxCenterY1 = alpha*curBoxCenterY1 + (1-alpha)* box.Center.Y;
					
					if(Mathf.Abs (oldBoxCenterX2-box2.Center.X)>jitterThres)
						curBoxCenterX2 = box2.Center.X;
					else
						curBoxCenterX2 = (box2.Center.X+curBoxCenterX2)/2.0f;
				//curBoxCenterX1 = alpha*curBoxCenterX1 + (1-alpha)*box.Center.X;
					if(Mathf.Abs (oldBoxCenterY2-box2.Center.Y)>jitterThres)
						curBoxCenterY2 = box2.Center.Y;
					else
						curBoxCenterY2 = (box2.Center.Y+curBoxCenterY2)/2.0f;
				//curBoxCenterY1 = alpha*curBoxCenterY1 + (1-alpha)* box.Center.Y;
					
					return;
				}
			#endregion
			if(fingerCounter2 >= 1)
			{
				handMode = handStates.TWOHANDSJOINED;
				fingerCounter2=0;	
				fingerCounter = 0;
				gesturesName.text = "Two Hands Joined";
			}
			else if(fingerCounter <= fingersClosed)
			{

				handMode = handStates.CLOSED;
				fingerCounter=0;
				gesturesName.text = "Fingers Closed";
			}
			else
			{
				handMode = handStates.OPEN;
				fingerCounter=0;
				gesturesName.text = "Fingers Opened";
			}
		
			}
			else{
				
					handMode = handStates.NONE;
					gesturesName.text = "None";
					return;
			}
		
			oldBoxCenterX1 = curBoxCenterX1;
			oldBoxCenterY1 = curBoxCenterY1;
			
			if(Mathf.Abs (oldBoxCenterX1-box.Center.X)>jitterThres)
				curBoxCenterX1 = box.Center.X;
			else
				curBoxCenterX1 = (box.Center.X+curBoxCenterX1)/2.0f;
				
			if(Mathf.Abs (oldBoxCenterY1-box.Center.Y)>jitterThres)
				curBoxCenterY1 = box.Center.Y;
			else
				curBoxCenterY1 = (box.Center.Y+curBoxCenterY1)/2.0f;
			
			if(handMode == handStates.TWOHANDSJOINED)
				curBoxCenterX1 += 30;
				
			
		}
	}
	
public int GetPalmPosX1()
	{
		return (int)((curBoxCenterX1 + cursorOffsetX) / depthImageWidth * Screen.width * cursorScaleX) ;
	}
	
	public int GetPalmPosY1()
	{
		return (int)((curBoxCenterY1 + cursorOffsetY) / depthImageHeight * Screen.height * cursorScaleY);
	}
	
	public Vector3 GetPalmPos1()
	{
		return new Vector3(GetPalmPosX1(), GetPalmPosY1(), 1);
	}
	
	public int GetPalmPosX2()
	{
		return (int)((curBoxCenterX2  + cursorOffsetX) / depthImageWidth * Screen.width * cursorScaleX);
	}
	
	public int GetPalmPosY2()
	{
		return (int)((curBoxCenterY2  + cursorOffsetY) / depthImageHeight * Screen.height * cursorScaleY);
	}
	
	public Vector3 GetPalmPos2()
	{
		return new Vector3(GetPalmPosX2(), GetPalmPosY2(), 1);
	}
	
	private Color32[] mipmapImg(Color32[] src, int width, int height)
	{
		int newWidth = width / 2;
		int newHeight = height / 2;
		Color32[] dst = new Color32[newWidth * newHeight];
		for(int yy = 0; yy < newHeight; yy++)
		{
			for(int xx = 0; xx < newWidth; xx++)
			{
				int TLidx = (xx * 2) + yy * 2 * width;
				int TRidx = (xx * 2 + 1) + yy * width * 2;
				int BLidx = (xx * 2) + (yy * 2 + 1) * width;
				int BRidx = (xx * 2 + 1) + (yy * 2 + 1) * width;
				dst[xx + yy * newWidth] = Color32.Lerp(Color32.Lerp(src[BLidx],src[BRidx],.5F),
				                                       Color32.Lerp(src[TLidx],src[TRidx],.5F),.5F);
			}
		}
		return dst;
	}
	void OnApplicationQuit()
	{
		Cv.ReleaseImage(img);
		Cv.ReleaseImage(imgGray);
		Cv.ReleaseImage(imgGrayDilated);
	}
}
